

#include "slep.h"
#include "hr_time.h"
#include "epph.h"
#include "Logistic.h"

FUNVAL mcLogisticCm1(double* A, double* y, double z, OPTS opt);
FUNVAL mcLogisticCm2(double* A, double* y, double z, OPTS opt);


FUNVAL mcLogisticC(double* A, double* y, double z, OPTS opt){
	if(opt.lFlag == 0 )
		return mcLogisticCm1(A,y,z,opt);
	else if(opt.lFlag == 1)
		return mcLogisticCm2(A,y,z,opt);
	else
		errorOut("Please set the values of opt.mFlag and opt.lFlag to correct values before executing mcLogisticC.");
}


FUNVAL mcLogisticCm1(double* A, double* y,double z,OPTS opt){
	FUNVAL ret;
	int m,n,k, iterStep,i, bFlag;
	double tmp1;
	double alpha,alphap,beta,L;
	double q, lambda;
	double fun_x,fun_s;
	double fVal,fValp,*funVal,*ValueL;
	double r_sum,l_sum;
	double *m1, *m2, *p_flag, *b, *ATb;
	double *x, *Ax;
	double *Axp, *xxp, *cp, *c, *ccp;
	double *s, *sc, *As, *aa, *bb, *prob, *gc;
	double *xp, *g, *v;
	double norm_xp, norm_xxp;
	double lambda0;
	int zf_step;

	m = opt.m;
	n = opt.n;
	k = opt.mc_k;
	if(z<= 0)
		errorOut("mcLogisticR: z should be positive");
	// sll_opts

	m1 = (double *) malloc(sizeof(double)*k);
	m2 = (double *) malloc(sizeof(double)*k);
	b = (double *) malloc(sizeof(double)*m*k);
	p_flag = (double *) malloc(sizeof(double)*m*k);
	ATb = (double *) malloc(sizeof(double)*n*k);
	Ax = (double *) malloc(sizeof(double)*m*k);
	x = (double *) malloc(sizeof(double)*n*k);
	c = (double *) malloc(sizeof(double)*k);
	xp = (double *) malloc(sizeof(double)*n*k);
	Axp = (double *) malloc(sizeof(double)*m*k);
	xxp = (double *) malloc(sizeof(double)*n*k);
	cp = (double *) malloc(sizeof(double)*k);
	ccp = (double *) malloc(sizeof(double)*k);
	sc = (double *) malloc(sizeof(double)*k);
	s = (double *) malloc(sizeof(double)*n*k);
	g = (double *) malloc(sizeof(double)*n*k);
	v = (double *) malloc(sizeof(double)*n*k);
	As = (double *) malloc(sizeof(double)*m*k);
	aa = (double *) malloc(sizeof(double)*m*k);
	bb = (double *) malloc(sizeof(double)*m*k);
	prob = (double *) malloc(sizeof(double)*m*k);
	gc = (double *) malloc(sizeof(double)*k);
	ValueL = (double *) malloc(sizeof(double)*opt.maxIter);
	funVal = (double *) malloc(sizeof(double)*opt.maxIter);
	
	initNormalization(&opt,A);

	if(!hasGot(&opt,"q"))
		q = opt.q = 2;
	else if ( (q=opt.q) ==2 )
		errorOut("mcLogisticR: q should be 2");

	// The following code assumes column major form
	memset(m1,0,sizeof(double)*k);
	memset(m2,0,sizeof(double)*k);
	for(i=0,tmp1=0;i<m*k;i++){
		p_flag[i] = ((y[i]>0.9999 && y[i]<1.0001)?1:0);
		tmp1 += p_flag[i];
		if(i%m == (m-1)){
			m1[i/m] = tmp1;
			m2[i/m] = m - m1[i/m];
			tmp1=0;
		}
	}

	if(opt.init == 2){
		memset(x,0,sizeof(double)*n*k);
		memset(c,0,sizeof(double)*k);
	}
	else{
		if(hasGot(&opt,"x0"))
			x = opt.x0;
		else
			memset(x,0,sizeof(double)*n*k);
		if(hasGot(&opt,"mc_c0"))
			c = opt.mc_c0;
		else
			for(i=0;i<k;i++)
				c[i] = log(m1[i]/m2[i]);
	}
	normalizedMM('N',m,n,k,A,x,Ax,&opt);

	if( opt.lFlag==0 ){
		lambda0 = 0 ;
		bFlag = 0;
		L = 1.0l/(m*k);
		
		//xp=x; Axp=Ax; xxp=zeros(n,k);
		//cp=c; ccp=zeros(1,k);
		dcopy(n*k,x,1,xp,1);
		dcopy(m*k,Ax,1,Axp,1);
		memset(xxp,0,sizeof(double)*n*k);
		dcopy(k,c,1,cp,1);
		memset(ccp,0,sizeof(double)*k);

		alphap = 0;
		alpha = 1;

		for(iterStep=0 ; iterStep < opt.maxIter; iterStep++){
			beta = (alphap-1)/alpha;
			//s=x + beta*xxp;
			//sc = c + beta * ccp;
			dcopy(n*k,x,1,s,1);
			daxpy(n*k,beta,xxp,1,s,1);
			dcopy(k,c,1,sc,1);
			daxpy(k,beta,ccp,1,sc,1);

			//As = Ax + beta*(Ax-Axp);
			dcopy(m*k,Ax,1,As,1);
			dscal(m*k,1+beta,As,1);
			daxpy(m*k,-beta,Axp,1,As,1);

			fun_s = 0;
			for(i=0;i<m*k;i++){
				aa[i] = -y[i] * (As[i] + sc[i/m]);
				bb[i] = maxof2(aa[i],0);
				fun_s += log( exp(-bb[i])+exp(aa[i]-bb[i]) ) + bb[i];
				prob[i] = 1/(1+exp(aa[i]));
				b[i] = -y[i] *(1-prob[i])/(m*k);
			}

			fun_s = fun_s/(m*k);
			for(i=0;i<k;i++)
				gc[i] = ddot(m,oneVector,1,&b[i*m],1);

			normalizedMM('T',m,n,k,A,b,g,&opt);
			//xp=x; Axp=Ax; cp=c;
			dcopy(n*k,x,1,xp,1);
			dcopy(m*k,Ax,1,Axp,1);
			dcopy(k,c,1,cp,1);

			for(;;){
				//v= s-g/L; c=sc-gc/L;
				dcopy(n*k,s,1,v,1);
				daxpy(n*k,-1/L,g,1,v,1);
				dcopy(k,sc,1,c,1);
				daxpy(k,-1/L,gc,1,c,1);

				ep21d(x,&lambda,&zf_step,v,n,k,z,lambda0);
				lambda0 = lambda;

				//v=x-s;
				dcopy(n*k,x,1,v,1);
				daxpy(n*k,-1,s,1,v,1);

				normalizedMM('N',m,n,k,A,x,Ax,&opt);

				fun_x=0;
				for(i=0;i<m*k;i++){
					aa[i] = -y[i] * (Ax[i] + c[i/m]);
					bb[i] = maxof2(aa[i],0);
					fun_x += log( exp(-bb[i])+exp(aa[i]-bb[i]) ) + bb[i];
				}
				fun_x = fun_x / (m*k);

				r_sum = (ddot(n*k,v,1,v,1) + ddot(k,c,1,c,1) 
					+ ddot(k,sc,1,sc,1) - 2*ddot(k,c,1,sc,1))/2;
				l_sum = fun_x - fun_s - ddot(n*k,v,1,g,1) 
					- ddot(k,c,1,gc,1) + ddot(k,sc,1,gc,1);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if(l_sum <= r_sum * L)
					break;
				else
					L = maxof2(2*L,l_sum/r_sum);
			}
			alphap = alpha;
			alpha = (1+sqrt(4*alpha*alpha+1))/2.0l;
			ValueL[iterStep] = L;

			//xxp = x -xp;
			//ccp = c -cp;
			dcopy(n*k,x,1,xxp,1);
			daxpy(n*k,-1,xp,1,xxp,1);
			dcopy(k,c,1,ccp,1);
			daxpy(k,-1,cp,1,ccp,1);

			fVal = fun_x;

			fValp = funVal[iterStep] = fVal;

			if(iterStep!=0)
				fValp = funVal[iterStep-1];

			if(bFlag)
				break;
			norm_xp = ddot(n*k,xp,1,xp,1);
			norm_xxp = ddot(n*k,xxp,1,xxp,1);
			if(terminationCondition(&opt,fVal,fValp,x,norm_xxp,norm_xp,iterStep))
				break;
		}
	}
	ret.funVal = funVal;
	ret.x = x;
	ret.errorCode = 0;
	ret.ValueL = ValueL;
	ret.totIter = iterStep;
	return ret;

}


FUNVAL mcLogisticCm2(double* A, double* y,double z,OPTS opt){
	FUNVAL ret;
	int m,n,k, iterStep,i, bFlag;
	double tmp1;
	double alpha,alphap,beta,L;
	double q, lambda;
	double fun_x,fun_s;
	double fVal,fValp,*funVal,*ValueL;
	double r_sum,l_sum;
	double *m1, *m2, *p_flag, *b, *ATb;
	double *x, *Ax;
	double *Axp, *xxp, *cp, *c, *ccp;
	double *s, *sc, *As, *aa, *bb, *prob, *gc;
	double *xp, *g, *v;
	double norm_xp, norm_xxp;
	double *xnew, *tnew, *Axnew, *cnew;
	double gamma, tao, lambda0;
	int zf_step;

	m = opt.m;
	n = opt.n;
	k = opt.mc_k;
	if(z<= 0)
		errorOut("mcLogisticR: z should be positive");
	// sll_opts

	m1 = (double *) malloc(sizeof(double)*k);
	m2 = (double *) malloc(sizeof(double)*k);
	b = (double *) malloc(sizeof(double)*m*k);
	p_flag = (double *) malloc(sizeof(double)*m*k);
	ATb = (double *) malloc(sizeof(double)*n*k);
	Ax = (double *) malloc(sizeof(double)*m*k);
	x = (double *) malloc(sizeof(double)*n*k);
	c = (double *) malloc(sizeof(double)*k);
	xp = (double *) malloc(sizeof(double)*n*k);
	Axp = (double *) malloc(sizeof(double)*m*k);
	xxp = (double *) malloc(sizeof(double)*n*k);
	cp = (double *) malloc(sizeof(double)*k);
	ccp = (double *) malloc(sizeof(double)*k);
	sc = (double *) malloc(sizeof(double)*k);
	s = (double *) malloc(sizeof(double)*n*k);
	g = (double *) malloc(sizeof(double)*n*k);
	v = (double *) malloc(sizeof(double)*n*k);
	As = (double *) malloc(sizeof(double)*m*k);
	aa = (double *) malloc(sizeof(double)*m*k);
	bb = (double *) malloc(sizeof(double)*m*k);
	prob = (double *) malloc(sizeof(double)*m*k);
	gc = (double *) malloc(sizeof(double)*k);
	ValueL = (double *) malloc(sizeof(double)*opt.maxIter);
	funVal = (double *) malloc(sizeof(double)*opt.maxIter);
	xnew = (double *) malloc(sizeof(double)*n*k);
	tnew = (double *) malloc(sizeof(double)*n);
	Axnew = (double *) malloc(sizeof(double)*m*k);
	cnew = (double *) malloc(sizeof(double)*k);
	
	initNormalization(&opt,A);

	if(!hasGot(&opt,"q"))
		q = opt.q = 2;
	else if ( (q=opt.q) == 2)
		errorOut("mcLogisticR: q should be 2");

	// The following code assumes column major form
	memset(m1,0,sizeof(double)*k);
	memset(m2,0,sizeof(double)*k);
	for(i=0,tmp1=0;i<m*k;i++){
		p_flag[i] = ((y[i]>0.9999 && y[i]<1.0001)?1:0);
		tmp1 += p_flag[i];
		if(i%m == (m-1)){
			m1[i/m] = tmp1;
			m2[i/m] = m - m1[i/m];
			tmp1=0;
		}
	}

	if(opt.init == 2){
		memset(x,0,sizeof(double)*n*k);
		for(i=0;i<k;i++)
			c[i] = log(m1[i]/m2[i]);
	}
	else{
		if(hasGot(&opt,"x0"))
			x = opt.x0;
		else
			memset(x,0,sizeof(double)*n*k);
		if(hasGot(&opt,"mc_c0"))
			c = opt.mc_c0;
		else
			for(i=0;i<k;i++)
				c[i] = log(m1[i]/m2[i]);
	}
	normalizedMM('N',m,n,k,A,x,Ax,&opt);

	if( opt.lFlag==1){
		lambda0 = 0;
		bFlag = 0;
		L = 1.0l/(m*k);

		gamma = 1;
		
		//xp=x; Axp=Ax; xxp=zeros(n,k);
		//cp=c; ccp=zeros(1,k);
		dcopy(n*k,x,1,xp,1);
		dcopy(m*k,Ax,1,Axp,1);
		memset(xxp,0,sizeof(double)*n*k);
		dcopy(k,c,1,cp,1);
		memset(ccp,0,sizeof(double)*k);

		for(iterStep=0 ; iterStep < opt.maxIter; iterStep++){
			for(;;){
				if(iterStep!=0){
					alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
					beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);

					//s=x + beta*xxp;
					//sc = c + beta * ccp;
					dcopy(n*k,x,1,s,1);
					daxpy(n*k,beta,xxp,1,s,1);
					dcopy(k,c,1,sc,1);
					daxpy(k,beta,ccp,1,sc,1);

					//As = Ax + beta*(Ax-Axp);
					dcopy(m*k,Ax,1,As,1);
					dscal(m*k,1+beta,As,1);
					daxpy(m*k,-beta,Axp,1,As,1);
				}else{
					alpha= (-1+ sqrt(5)) / 2;  
					beta=0; 
					//s=x; As=Ax; sc=c;
					dcopy(n*k,x,1,s,1);
					dcopy(m*k,Ax,1,As,1);
					dcopy(k,c,1,sc,1);
				}

				fun_s = 0;
				for(i=0;i<m*k;i++){
					aa[i] = -y[i] * (As[i] + sc[i/m]);
					bb[i] = maxof2(aa[i],0);
					fun_s += log( exp(-bb[i])+exp(aa[i]-bb[i]) ) + bb[i];
					prob[i] = 1/(1+exp(aa[i]));
					b[i] = -y[i] *(1-prob[i])/(m*k);
				}

				fun_s = fun_s/(m*k);
				for(i=0;i<k;i++)
					gc[i] = ddot(m,oneVector,1,&b[i*m],1);

				normalizedMM('T',m,n,k,A,b,g,&opt);

				//v= s-g/L; cnew=sc-gc/L;
				dcopy(n*k,s,1,v,1);
				daxpy(n*k,-1/L,g,1,v,1);
				dcopy(k,sc,1,cnew,1);
				daxpy(k,-1/L,gc,1,cnew,1);

				ep21d(x,&lambda,&zf_step,v,n,k,z,lambda0);
				lambda0 = lambda;

				//v=xnew-s;
				dcopy(n*k,xnew,1,v,1);
				daxpy(n*k,-1,s,1,v,1);

				normalizedMM('N',m,n,k,A,xnew,Axnew,&opt);

				fun_x=0;
				for(i=0;i<m*k;i++){
					aa[i] = -y[i] * (Axnew[i] + cnew[i/m]);
					bb[i] = maxof2(aa[i],0);
					fun_x += log( exp(-bb[i])+exp(aa[i]-bb[i]) ) + bb[i];
				}
				fun_x = fun_x / (m*k);

				r_sum = (ddot(n*k,v,1,v,1) + ddot(k,cnew,1,cnew,1) 
					+ ddot(k,sc,1,sc,1) - 2*ddot(k,cnew,1,sc,1))/2;
				l_sum = fun_x - fun_s - ddot(n*k,v,1,g,1) 
					- ddot(k,cnew,1,gc,1) + ddot(k,sc,1,gc,1);

				if(r_sum <= 1e-20){
					bFlag=1;
					break;
				}

				if(l_sum <= r_sum * L)
					break;
				else
					L = maxof2(2*L,l_sum/r_sum);
			}

			gamma=L* alpha* alpha;
			alphap=alpha;

			ValueL[iterStep] = L;

			tao=L * r_sum / l_sum;
			if (tao >=5)
				L=L*0.8;

			//xp=x;   x=xnew; 
			dcopy(n*k,x,1,xp,1);
			dcopy(n*k,xnew,1,x,1);
			dcopy(n*k,x,1,xxp,1);

			//Axp=Ax; Ax=Axnew;
			dcopy(m*k,Ax,1,Axp,1);
			dcopy(m*k,Axnew,1,Ax,1);

			//cp=c; c=cnew;
			dcopy(k,c,1,cp,1);
			dcopy(k,cnew,1,c,1);

			//xxp = x-xp;
			//ccp = c-cp;
			dcopy(n*k,x,1,xxp,1);
			daxpy(n*k,-1,xp,1,xxp,1);
			dcopy(k,c,1,ccp,1);
			daxpy(k,-1,cp,1,ccp,1);

			fVal = fun_x ;

			fValp = funVal[iterStep] = fVal;

			if(iterStep!=0)
				fValp = funVal[iterStep-1];

			if(bFlag)
				break;
			norm_xp = ddot(n*k,xp,1,xp,1);
			norm_xxp = ddot(n*k,xxp,1,xxp,1);
			if(terminationCondition(&opt,fVal,fValp,x,norm_xxp,norm_xp,iterStep))
				break;
		}
	}
	ret.funVal = funVal;
	ret.x = x;
	ret.errorCode = 0;
	ret.ValueL = ValueL;
	ret.totIter = iterStep;
	return ret;

}





